#
# Copyright (C) 2023, Inria
# GRAPHDECO research group, https://team.inria.fr/graphdeco
# All rights reserved.
#
# This software is free for non-commercial, research and evaluation use 
# under the terms of the LICENSE.md file.
#
# For inquiries contact  george.drettakis@inria.fr
#

import torch
from functools import reduce
import numpy as np
import open3d as o3d
from tqdm import tqdm
from utils.general_utils import inverse_sigmoid, get_expon_lr_func
from torch import nn
import os, math
import faiss
from einops import repeat
from utils.system_utils import mkdir_p
from plyfile import PlyData, PlyElement
from simple_knn._C import distCUDA2
from utils.graphics_utils import BasicPointCloud
from utils.general_utils import strip_symmetric, build_scaling_rotation
from scene.embedding import Embedding, MLP, PosEmbedding
from arguments import ModelParams, OptimizationParams
from diff_gaussian_rasterization import GaussianRasterizationSettings, GaussianRasterizer
from .cameras import Camera


def save_ply(points, path):
    points = points.detach().clone().cpu().numpy()
    
    dtype = [('x', 'f4'), ('y', 'f4'), ('z', 'f4'),
            ('nx', 'f4'), ('ny', 'f4'), ('nz', 'f4'),
            ('red', 'u1'), ('green', 'u1'), ('blue', 'u1')]
    
    normals = np.zeros_like(points)
    rgb = np.zeros_like(points)

    elements = np.empty(points.shape[0], dtype=dtype)
    attributes = np.concatenate((points, normals, rgb), axis=1)
    elements[:] = list(map(tuple, attributes))

    # Create the PlyData object and write to file
    vertex_element = PlyElement.describe(elements, 'vertex')
    ply_data = PlyData([vertex_element])
    ply_data.write(path)
    
    
def visible_filter(points, camera: Camera):
    tanfovx = math.tan(camera.FoVx * 0.5)
    tanfovy = math.tan(camera.FoVy * 0.5)
    
    raster_settings = GaussianRasterizationSettings(
                        image_height=int(camera.image_height),
                        image_width=int(camera.image_width),
                        tanfovx=tanfovx,
                        tanfovy=tanfovy,
                        bg=torch.tensor([0, 0, 0], dtype=torch.float32, device=points.device),
                        scale_modifier=1.0,
                        viewmatrix=camera.world_view_transform,
                        projmatrix=camera.full_proj_transform,
                        sh_degree=3,
                        campos=camera.camera_center,
                        prefiltered=False,
                        debug=False)

    rasterizer = GaussianRasterizer(raster_settings=raster_settings)
    visible_mask = rasterizer.markVisible(points)
    return visible_mask

    
class GaussianModel:

    def setup_functions(self):
        def build_covariance_from_scaling_rotation(scaling, scaling_modifier, rotation):
            L = build_scaling_rotation(scaling_modifier * scaling, rotation)
            actual_covariance = L @ L.transpose(1, 2)
            symm = strip_symmetric(actual_covariance)
            return symm
        
        self.scaling_activation = torch.exp
        self.scaling_inverse_activation = torch.log
        self.covariance_activation = build_covariance_from_scaling_rotation
        self.opacity_activation = torch.sigmoid
        self.inverse_opacity_activation = inverse_sigmoid
        self.rotation_activation = torch.nn.functional.normalize


    def __init__(self, dataset: ModelParams):

        self.feat_dim = dataset.feat_dim
        self.hid_dim = dataset.hid_dim
        self.n_layers = dataset.n_layers
        self.init_points_num = dataset.init_points_num
        self.n_offsets = dataset.n_offsets
        self.voxel_size = dataset.voxel_size

        self.ratio = dataset.ratio
        self.scale_dim = dataset.scale_dim
        self.scaling_mode = dataset.scaling_mode
        self.sampling_mode = dataset.sampling_mode
        self.levels = dataset.levels
        self.add_neural_feat = dataset.add_neural_feat
        
        self._anchor = torch.empty(0)
        self._anchor_feat = torch.empty(0)
        self._scaling = torch.empty(0)
                        
        self.optimizer = None
        self.spatial_lr_scale = 0
        self.setup_functions()
        
        self.dir_freqs = 4
        self.dir_embedder = PosEmbedding(N_freqs=self.dir_freqs).cuda() 
        self.feat_in_dim = self.feat_dim + 3 * (2 * self.dir_freqs + 1)
        self.init_encoders()
        
        
    def init_encoders(self):
        self.mlp_opacity = MLP(self.feat_in_dim, self.hid_dim, self.n_offsets*1, n_layers=self.n_layers, out_act=nn.Tanh()).cuda()
        self.mlp_cov     = MLP(self.feat_in_dim, self.hid_dim, self.n_offsets*7, n_layers=self.n_layers, out_act=None).cuda()
        self.mlp_color   = MLP(self.feat_in_dim, self.hid_dim, self.n_offsets*3, n_layers=self.n_layers, out_act=nn.Sigmoid()).cuda()
        self.mlp_offset  = MLP(self.feat_dim, self.hid_dim, self.n_offsets*3, n_layers=self.n_layers, out_act=None).cuda()
    
    
    def eval(self):
        self.mlp_opacity.eval()
        self.mlp_cov.eval()
        self.mlp_color.eval()
        self.mlp_offset.eval()


    def train(self):
        self.mlp_opacity.train()
        self.mlp_cov.train()
        self.mlp_color.train()
        self.mlp_offset.train()
        
        
    @property
    def get_scaling(self):
        return self.scaling_activation(self._scaling)
        
    
    @property
    def get_anchor(self):
        return self._anchor
    
    
    @property
    def get_anchor_feat(self):
        return self._anchor_feat
        
    
    @property
    def get_opacity_mlp(self):
        return self.mlp_opacity
        
    
    @property
    def get_cov_mlp(self):
        return self.mlp_cov
        
    
    @property 
    def get_color_mlp(self):
        return self.mlp_color
        
    
    @property
    def get_offset_mlp(self):
        return self.mlp_offset                
            
            
    def voxelize_sample(self, data=None, voxel_size=0.01):
        np.random.shuffle(data)
        data = np.unique(np.round(data / voxel_size), axis=0) * voxel_size
        return data
    
    
    def get_voxelize_mask(self, points):
        points = points.detach().clone().cpu().numpy()
        _, unique_idx = np.unique(np.round(points / self.voxel_size), axis=0, return_index=True)
        return unique_idx
    

    def create_from_pcd(self, pcd: BasicPointCloud, spatial_lr_scale: float):
        self.spatial_lr_scale = spatial_lr_scale
        points = pcd.points
        
        if self.ratio > 1:
            self.ratio = min(self.ratio, int(points.shape[0] / self.init_points_num))
            print(f"Downsampling ratio: {self.ratio}")
            if self.sampling_mode == 'uniform':
                points = points[::self.ratio]
            elif self.sampling_mode == 'cluster':   
                points = np.ascontiguousarray(points)
                N = int(points.shape[0] // self.ratio)
                                
                kmeans = faiss.Kmeans(d=points.shape[1], k=N, niter=30, verbose=True, gpu=True)
                kmeans.train(points)
                points = kmeans.centroids
            else:
                raise ValueError(f"{self.sampling_method} is not supported!")
        
        if self.voxel_size <= 0:
            init_points = torch.tensor(points).float().cuda()
            init_dist = distCUDA2(init_points).float().cuda()
            median_dist, _ = torch.kthvalue(init_dist, int(init_dist.shape[0]*0.5))
            self.voxel_size = median_dist.item()
            del init_dist
            del init_points
            torch.cuda.empty_cache()

        print(f'Initial voxel_size: {self.voxel_size}')
        points = self.voxelize_sample(points, voxel_size=self.voxel_size)
        
        fused_point_cloud = torch.tensor(np.asarray(points)).float().cuda()  
        anchors_feat = torch.zeros((fused_point_cloud.shape[0], self.feat_dim)).float().cuda()
        dist2 = torch.clamp_min(distCUDA2(fused_point_cloud).float().cuda(), 0.0000001)
        scaling = torch.log(torch.sqrt(dist2))[..., None]
        assert self.scale_dim in [1, 2, 3, 6], "Scaling only supports [1, 2, 3, 6]"
        
        self._anchor = nn.Parameter(fused_point_cloud.requires_grad_(True))
        self._anchor_feat = nn.Parameter(anchors_feat.requires_grad_(True))
        self._scaling = nn.Parameter(scaling.repeat(1, self.scale_dim).requires_grad_(True))
        torch.cuda.empty_cache()
        print("Number of points at level-0 initialisation: ", fused_point_cloud.shape[0])
                

    def training_setup(self, training_args: OptimizationParams, level=0):
        self.opacity_accum = torch.zeros((self.get_anchor.shape[0]*self.n_offsets, 1), device="cuda")
        self.demon = torch.zeros((self.get_anchor.shape[0]*self.n_offsets, 1), device="cuda")
        self.scaling_lr = training_args.scaling_lr
        
        l = [
            {'params': self.mlp_opacity.parameters(), 'lr': training_args.mlp_opacity_lr_init, "name": "mlp_opacity"},
            {'params': self.mlp_cov.parameters(), 'lr': training_args.mlp_cov_lr_init, "name": "mlp_cov"},
            {'params': self.mlp_color.parameters(), 'lr': training_args.mlp_color_lr_init, "name": "mlp_color"},
            {'params': self.mlp_offset.parameters(), 'lr': training_args.mlp_offset_lr_init, "name": "mlp_offset"},
            
            {'params': self._anchor, 'lr': training_args.position_lr_init*self.spatial_lr_scale, "name": "anchor"},
            {'params': self._anchor_feat, 'lr': training_args.feature_lr, "name": "anchor_feat"},
            {'params': self._scaling, 'lr': training_args.scaling_lr, "name": "scaling"},
            ]
        
        self.optimizer = torch.optim.Adam(l, lr=0.0, eps=1e-15)
        
        self.mlp_opacity_scheduler_args = get_expon_lr_func(lr_init=training_args.mlp_opacity_lr_init,
                                                    lr_final=training_args.mlp_opacity_lr_final,
                                                    lr_delay_mult=training_args.mlp_opacity_lr_delay_mult,
                                                    max_steps=training_args.mlp_opacity_lr_max_steps)
        
        self.mlp_offset_scheduler_args = get_expon_lr_func(lr_init=training_args.mlp_offset_lr_init,
                                                    lr_final=training_args.mlp_offset_lr_final,
                                                    lr_delay_mult=training_args.mlp_offset_lr_delay_mult,
                                                    max_steps=training_args.mlp_offset_lr_max_steps)
        
        self.mlp_cov_scheduler_args = get_expon_lr_func(lr_init=training_args.mlp_cov_lr_init,
                                                    lr_final=training_args.mlp_cov_lr_final,
                                                    lr_delay_mult=training_args.mlp_cov_lr_delay_mult,
                                                    max_steps=training_args.mlp_cov_lr_max_steps)
        
        self.mlp_color_scheduler_args = get_expon_lr_func(lr_init=training_args.mlp_color_lr_init,
                                                    lr_final=training_args.mlp_color_lr_final,
                                                    lr_delay_mult=training_args.mlp_color_lr_delay_mult,
                                                    max_steps=training_args.mlp_color_lr_max_steps)


    def update_learning_rate(self, iteration):
        ''' Learning rate scheduling per step '''
        for param_group in self.optimizer.param_groups:
            if param_group["name"] == "mlp_opacity":
                lr = self.mlp_opacity_scheduler_args(iteration)
                param_group['lr'] = lr 
            if param_group["name"] == "mlp_cov":
                lr = self.mlp_cov_scheduler_args(iteration)
                param_group['lr'] = lr 
            if param_group["name"] == "mlp_color":
                lr = self.mlp_color_scheduler_args(iteration)
                param_group['lr'] = lr 
            if param_group["name"] == "mlp_offset":
                lr = self.mlp_offset_scheduler_args(iteration)
                param_group['lr'] = lr     
                                
    
    def construct_list_of_attributes(self):
        l = ['x', 'y', 'z']
        for i in range(self._anchor_feat.shape[1]):
            l.append(f'anchor_feat_{i}')
        for i in range(self._scaling.shape[1]):
            l.append(f"scaling_{i}")
        return l
             
    
    def save_ply(self, path):
        mkdir_p(os.path.dirname(path))
        anchor = self._anchor.detach().cpu().numpy()
        anchor_feat = self._anchor_feat.detach().cpu().numpy()
        scaling = self._scaling.detach().cpu().numpy()
        dtype_full = [(attribute, 'f4') for attribute in self.construct_list_of_attributes()]
        elements = np.empty(anchor.shape[0], dtype=dtype_full)
        attributes = np.concatenate((anchor, anchor_feat, scaling), axis=1)
        elements[:] = list(map(tuple, attributes))
        el = PlyElement.describe(elements, 'vertex')
        PlyData([el]).write(path)
        

    def load_ply(self, path):
        plydata = PlyData.read(path)
        anchor = np.stack((np.asarray(plydata.elements[0]["x"]),
                        np.asarray(plydata.elements[0]["y"]),
                        np.asarray(plydata.elements[0]["z"])),  axis=1).astype(np.float32)
        self._anchor = nn.Parameter(torch.tensor(anchor).float().cuda().requires_grad_(True))
        
        anchor_feat_names = [p.name for p in plydata.elements[0].properties if p.name.startswith("anchor_feat")]
        anchor_feat_names = sorted(anchor_feat_names, key = lambda x: int(x.split('_')[-1]))
        anchor_feats = np.zeros((anchor.shape[0], len(anchor_feat_names)))
        for idx, attr_name in enumerate(anchor_feat_names):
            anchor_feats[:, idx] = np.asarray(plydata.elements[0][attr_name]).astype(np.float32)
        self._anchor_feat = nn.Parameter(torch.tensor(anchor_feats).float().cuda().requires_grad_(True))
        
        scaling_names = [p.name for p in plydata.elements[0].properties if p.name.startswith("scaling")]
        scaling_names = sorted(scaling_names, key = lambda x: int(x.split('_')[-1]))
        scaling = np.zeros((anchor.shape[0], len(scaling_names)))
        for idx, attr_name in enumerate(scaling_names):
            scaling[:, idx] = np.asarray(plydata.elements[0][attr_name]).astype(np.float32)
        self._scaling = nn.Parameter(torch.tensor(scaling).float().cuda().requires_grad_(True))
                
        torch.cuda.empty_cache()
        print(f"Loading *.ply from {path} successfully !")
    
    
    @torch.no_grad()
    def generate_level_anchor(self, train_cameras):
        anchor = self.get_anchor
        anchor_feat = self.get_anchor_feat
        opacity = torch.zeros((anchor.shape[0], self.n_offsets), device=anchor.device)  # (N, k)
        counts = torch.zeros((anchor.shape[0], self.n_offsets), device=anchor.device)   # (N, k)
        
        for camera in tqdm(train_cameras, desc=f"preprocess level gaussians..."):
            visible_mask = visible_filter(anchor, camera)
            view_anchor, view_anchor_feat = anchor[visible_mask], anchor_feat[visible_mask]
            
            ob_view = view_anchor - camera.camera_center            # (N, 3)
            ob_dist = ob_view.norm(dim=1, keepdim=True)             # (N, 1)
            ob_view = ob_view / ob_dist                             # (N, 3)
            embed_view = self.dir_embedder(ob_view) 
            
            cat_local_view = torch.cat([view_anchor_feat, embed_view], dim=1)
            neural_opacity = self.get_opacity_mlp(cat_local_view)            # [N, k]
                        
            opacity[visible_mask] += neural_opacity
            counts[visible_mask] += 1
            
        mean_opacity = (opacity / counts).reshape(-1)
        mean_opacity[mean_opacity.isnan()] = 0.0
        return mean_opacity > 0.005
            
    
    @torch.no_grad()
    def preprocess_level_gaussians(self, train_cameras=None, model_path=""):
        cur_level = int(model_path[-1])
        pre_level_ply_path = os.path.join(model_path[:-1] + str(cur_level-1), f"level_anchor_{cur_level-1}.ply")
        save_ply(self.get_anchor, pre_level_ply_path)

        anchor = self.get_anchor
        anchor_feat = self.get_anchor_feat
        grid_scaling = self.get_scaling
        repeat_anchor = repeat(anchor, 'n (c) -> (n k) (c)', k=self.n_offsets)                  # (N*k, 3)
        repeat_scaling = repeat(grid_scaling, 'n (c) -> (n k) (c)', k=self.n_offsets)           # (N*k, 1)
        
        if self.scale_dim == 1 or self.scale_dim == 3:
            repeat_offset_scaling = repeat_scaling
        elif self.scale_dim == 2:
            repeat_offset_scaling = repeat_scaling[:, :1]
        elif self.scale_dim == 6:
            repeat_offset_scaling = repeat_scaling[:, :3]
        
        valid_mask = self.generate_level_anchor(train_cameras)
        offsets = self.get_offset_mlp(anchor_feat).reshape(-1, 3)
        xyz = (repeat_anchor + offsets * repeat_offset_scaling)[valid_mask]
        anchor = torch.unique(torch.round(xyz/self.voxel_size), dim=0) * self.voxel_size
        
        self._anchor = nn.Parameter(anchor.float().cuda().requires_grad_(True))
        self._anchor_feat = nn.Parameter(torch.zeros((anchor.shape[0], self.feat_dim)).float().cuda().requires_grad_(True))
        dist2 = torch.clamp_min(distCUDA2(self._anchor).float().cuda(), 0.0000001)
        self._scaling = nn.Parameter(torch.log(torch.sqrt(dist2))[..., None].repeat(1, self.scale_dim).requires_grad_(True))
        self.init_encoders()
        save_ply(anchor, os.path.join(model_path, f"level_anchor_{cur_level}.ply"))
        torch.cuda.empty_cache()
        print(f"Number of points at level-{cur_level} initialisation: ", anchor.shape[0])
        

    def replace_tensor_to_optimizer(self, tensor, name):
        optimizable_tensors = {}
        for group in self.optimizer.param_groups:
            if "mlp" in group['name']:
                continue
            if group["name"] == name:
                stored_state = self.optimizer.state.get(group['params'][0], None)
                stored_state["exp_avg"] = torch.zeros_like(tensor)
                stored_state["exp_avg_sq"] = torch.zeros_like(tensor)
                
                del self.optimizer.state[group['params'][0]]
                group["params"][0] = nn.Parameter(tensor.requires_grad_(True))
                self.optimizer.state[group['params'][0]] = stored_state
                optimizable_tensors[group["name"]] = group["params"][0]
        return optimizable_tensors


    def cat_tensors_to_optimizer(self, tensors_dict):
        optimizable_tensors = {}
        for group in self.optimizer.param_groups:
            if "mlp" in group['name']:
                continue
            assert len(group["params"]) == 1
            extension_tensor = tensors_dict[group["name"]]
            stored_state = self.optimizer.state.get(group['params'][0], None)
            if stored_state is not None:
                stored_state["exp_avg"] = torch.cat((stored_state["exp_avg"], torch.zeros_like(extension_tensor)), dim=0)
                stored_state["exp_avg_sq"] = torch.cat((stored_state["exp_avg_sq"], torch.zeros_like(extension_tensor)), dim=0)

                del self.optimizer.state[group['params'][0]]
                group["params"][0] = nn.Parameter(torch.cat((group["params"][0], extension_tensor), dim=0).requires_grad_(True))
                self.optimizer.state[group['params'][0]] = stored_state

                optimizable_tensors[group["name"]] = group["params"][0]
            else:
                group["params"][0] = nn.Parameter(torch.cat((group["params"][0], extension_tensor), dim=0).requires_grad_(True))
                optimizable_tensors[group["name"]] = group["params"][0]
        return optimizable_tensors
    
    
    def _prune_anchor_optimizer(self, mask):
        optimizable_tensors = {}
        for group in self.optimizer.param_groups:
            if  'mlp' in group['name']:
                continue
            stored_state = self.optimizer.state.get(group['params'][0], None)
            if stored_state is not None:
                stored_state["exp_avg"] = stored_state["exp_avg"][mask]
                stored_state["exp_avg_sq"] = stored_state["exp_avg_sq"][mask]

                del self.optimizer.state[group['params'][0]]
                group["params"][0] = nn.Parameter((group["params"][0][mask].requires_grad_(True)))
                self.optimizer.state[group['params'][0]] = stored_state
                optimizable_tensors[group["name"]] = group["params"][0]
            else:
                group["params"][0] = nn.Parameter(group["params"][0][mask].requires_grad_(True))
                optimizable_tensors[group["name"]] = group["params"][0]
        return optimizable_tensors


    def training_statis(self, opacity, visibility_filter, selection_mask, visible_mask):
        tem_anchor_mask = repeat(visible_mask, 'n -> (n k)', k=self.n_offsets)

        tem_selection_mask = torch.zeros_like(selection_mask).bool()
        tem_selection_mask[selection_mask] = visibility_filter
        selection_mask = torch.logical_and(tem_selection_mask, selection_mask)
        
        tem_mask = torch.zeros_like(tem_anchor_mask).bool()
        tem_mask[tem_anchor_mask] = selection_mask
        mask = torch.logical_and(tem_anchor_mask, tem_mask)
        
        self.opacity_accum[mask] += opacity[visibility_filter]
        self.demon[mask] += 1       
        del tem_anchor_mask, selection_mask, tem_mask, mask
        
        
    def adjust_anchor(self, min_opacity=0.005):
        mean_opacity = self.opacity_accum / self.demon      # (N*k, 1)
        mean_opacity[mean_opacity.isnan()] = 0.0
        anchor_mean_opacity = mean_opacity.reshape(-1, self.n_offsets).mean(-1) # (N,)
        
        prune_mask = (anchor_mean_opacity < min_opacity)
        self.prune_anchor(prune_mask)
        
        self.opacity_accum = torch.zeros((self.get_anchor.shape[0]*self.n_offsets, 1), device="cuda")
        self.demon = torch.zeros((self.get_anchor.shape[0]*self.n_offsets, 1), device="cuda")
        torch.cuda.empty_cache()


    def prune_anchor(self, prune_mask):
        valid_points_mask = ~prune_mask
        optimizable_tensors = self._prune_anchor_optimizer(valid_points_mask)
        self._anchor = optimizable_tensors["anchor"]
        self._anchor_feat = optimizable_tensors["anchor_feat"]
        
        if self.scaling_lr > 1e-6:
            self._scaling = optimizable_tensors["scaling"]
        else:
            dist2 = torch.clamp_min(distCUDA2(self._anchor).float().cuda(), 0.0000001)
            self._scaling = nn.Parameter(torch.log(torch.sqrt(dist2))[..., None].repeat(1, self.scale_dim).requires_grad_(True))
    
          
    def save_pth(self, path):
        mkdir_p(os.path.dirname(path))
        ckpt = {
                'opacity_mlp': self.mlp_opacity.state_dict(),
                'cov_mlp': self.mlp_cov.state_dict(),
                'color_mlp': self.mlp_color.state_dict(),
                'offset_mlp': self.mlp_offset.state_dict(),
            }
        torch.save(ckpt, path)


    def load_pth(self, path):
        checkpoint = torch.load(path, map_location='cuda')
        self.mlp_opacity.load_state_dict(checkpoint['opacity_mlp'])
        self.mlp_cov.load_state_dict(checkpoint['cov_mlp'])
        self.mlp_color.load_state_dict(checkpoint['color_mlp'])
        self.mlp_offset.load_state_dict(checkpoint['offset_mlp'])
        print(f"Loading *.pth from {path} successfully !")
        
